#include <cassert>
#include <boost/algorithm/string.hpp>
#include <map>

#include "../../../XMLTools.h"

#include "Force3DSensor.h"

using namespace MMM;

Force3DSensorPtr Force3DSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    Force3DSensorPtr sensor(new Force3DSensor());
    sensor->loadSensor(node);
    return sensor;
}

Force3DSensor::Force3DSensor() : AttachedSensor()
{
}

Force3DSensor::Force3DSensor(const std::string &segment, const std::string &offsetUnit, const Eigen::Matrix4f &offset, const std::string &description) :
    AttachedSensor(segment, offsetUnit, offset, description)
{
}

boost::shared_ptr<BasicSensor<Force3DSensorMeasurement> > Force3DSensor::cloneConfiguration() {
    Force3DSensorPtr m(new Force3DSensor(segment, offsetUnit, offset, description));
    return m;
}

void Force3DSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    RapidXMLReaderNodePtr forceNode = node->first_node("Force");
    Force3DSensorMeasurementPtr m(new Force3DSensorMeasurement(timestep, XML::readFloatArray(forceNode->value(), 3), type));
    addSensorMeasurement(m);
}

std::string Force3DSensor::getType() {
    return TYPE;
}

std::string Force3DSensor::getVersion() {
    return VERSION;
}

int Force3DSensor::getPriority() {
    return 60;
}
